//-----------------------------------------------------------------------------
//
// @file	Vector.cpp
// @brief	Vector  classes (code modified from DonHouse, Texas A&M Univ)
// @author	Vinod Melapudi
// @date	20Feb2010
//
//-----------------------------------------------------------------------------

#include <iomanip>
#include <iostream>
#include "Vector.h" 

using namespace std;



Vector2::Vector2(float vx, float vy){ 
	set(vx, vy); 
} 

Vector2::Vector2(const Vector2 &v){ 
	set(v); 
} 

Vector3::Vector3(float vx, float vy, float vz){ 
	set(vx, vy, vz); 
} 

Vector3::Vector3(const Vector3 &v){ 
	set(v); 
} 

Vector3::Vector3(const Vector2 &v){ 
	set(v.x, v.y); 
} 

Vector4::Vector4(float vx, float vy, float vz, float vw){ 
	set(vx, vy, vz, vw); 
} 

Vector4::Vector4(const Vector4 &v){ 
	set(v); 
} 

Vector4::Vector4(const Vector3 &v){ 
	set(v.x, v.y, v.z, 1.f); 
} 

Vector::Vector(int vN, float *vx){ 
	int i; 

	setsize(vN); 
	if(vx != NULL) 
		for(i = 0; i < N; i++) 
			v[i] = vx[i]; 
} 

Vector::Vector(const Vector& V){ 
	set(V); 
} 

Vector::Vector(float vx, float vy){ 
	setsize(2); 
	set(vx, vy); 
} 

Vector::Vector(float vx, float vy, float vz){ 
	setsize(3); 
	set(vx, vy, vz); 
} 

Vector::Vector(float vx, float vy, float vz, float vw){ 
	setsize(4); 
	set(vx, vy, vz, vw); 
} 

// Destructor 
Vector::~Vector(){ 
	delete []v; 
} 

// 
// Array index form of a vector 
// Routines returning an lvalue: i.e. X[i] returns addr of X[i] 
// 
float& Vector2::operator[](int i) 
{ 
	if(i < 0 || i > 1){ 
		cerr << "2D vector index bounds error" << endl; 
		exit(1); 
	} 

	switch(i){ 
  case 0: 
	  return x; 
  default: 
	  return y; 
	} 
} 

float& Vector3::operator[](int i) 
{ 
	if(i < 0 || i > 2){ 
		cerr << "3D vector index bounds error" << endl; 
		exit(1); 
	} 

	switch(i){ 
  case 0: 
	  return x; 
  case 1: 
	  return y; 
  default: 
	  return z; 
	} 
} 

float& Vector4::operator[](int i) 
{ 
	if(i < 0 || i > 3){ 
		cerr << "4D vector index bounds error" << endl; 
		exit(1); 
	} 

	switch(i){ 
  case 0: 
	  return x; 
  case 1: 
	  return y; 
  case 2: 
	  return z; 
  default: 
	  return w; 
	} 
} 

float& Vector::operator[](int i) 
{ 
	if(i < 0 || i >= N){ 
		cerr << N << " vector index bounds error" << endl; 
		exit(1); 
	} 

	return v[i]; 
} 

// 
// Array index form of a vector 
// Routines returning an rvalue: i.e. X[i] returns contents of X[i] 
// 
const float& Vector2::operator[](int i) const 
{ 
	if(i < 0 || i > 1){ 
		cerr << "2D vector index bounds error" << endl; 
		exit(1); 
	} 

	switch(i){ 
  case 0: 
	  return x; 
  default: 
	  return y; 
	} 
} 

const float& Vector3::operator[](int i) const 
{ 
	if(i < 0 || i > 2){ 
		cerr << "3D vector index bounds error" << endl; 
		exit(1); 
	} 

	switch(i){ 
  case 0: 
	  return x; 
  case 1: 
	  return y; 
  default: 
	  return z; 
	} 
} 

const float& Vector4::operator[](int i) const 
{ 
	if(i < 0 || i > 3){ 
		cerr << "4D vector index bounds error" << endl; 
		exit(1); 
	} 

	switch(i){ 
  case 0: 
	  return x; 
  case 1: 
	  return y; 
  case 2: 
	  return z; 
  default: 
	  return w; 
	} 
} 

const float& Vector::operator[](int i) const 
{ 
	if(i < 0 || i >= N){ 
		cerr << N << " vector index bounds error" << endl; 
		exit(1); 
	} 

	return v[i]; 
} 

// Set size of generic Vector 
void Vector::setsize(int vN){ 
	if(vN < 0){ 
		cerr << "vector dimension of " << vN << " invalid" << endl; 
		exit(1);     
	} 

	N = vN; 
	if(N == 0) 
		v = NULL; 
	else{ 
		v = new float[N]; 
		if(!v){ 
			cerr << "not enough memory to allocate vector of size " << N << endl; 
			exit(1);     
		} 
	} 

	int i; 
	for(i = 0; i < N; i++) 
		v[i] = 0; 
} 

// Projections of vectors into other dimensions 
Vector2::operator Vector3(){ 
	Vector3 v1(x, y, 0); 
	return v1; 
} 

Vector2::operator Vector4(){ 
	Vector4 v1(x, y, 0, 0); 
	return v1; 
} 

Vector2::operator Vector(){ 
	Vector v1(x, y); 
	return v1; 
} 

Vector3::operator Vector4(){ 

	Vector4 v1(x, y, z, 0); 
	return v1; 
} 

Vector3::operator Vector(){ 
	Vector v1(x, y, z); 
	return v1; 
} 

Vector4::operator Vector(){ 
	Vector v1(x, y, z, w); 
	return v1; 
} 

Vector::operator Vector2(){ 
	if(N > 2){ 
		cerr << "cannot convert " << N << " Vector to Vector2" << endl; 
		exit(1);     
	} 

	Vector2 rv; 
	int i; 
	for(i = 0; i < N; i++) 
		rv[i] = v[i]; 
	for(; i < 2; i++) 
		rv[i] = 0; 
	return rv; 
} 

Vector::operator Vector3(){ 
	if(N > 3){ 
		cerr << "cannot convert " << N << " Vector to Vector3" << endl; 
		exit(1);     
	} 

	Vector3 rv; 
	int i; 
	for(i = 0; i < N; i++) 
		rv[i] = v[i]; 
	for(; i < 3; i++) 
		rv[i] = 0; 
	return rv; 
} 

Vector::operator Vector4(){ 
	if(N > 4){ 
		cerr << "cannot convert " << N << " Vector to Vector4" << endl; 
		exit(1);     
	} 

	Vector4 rv; 
	int i; 
	for(i = 0; i < N; i++) 
		rv[i] = v[i]; 
	for(; i < 4; i++) 
		rv[i] = 0; 
	return rv; 
} 

int Vector::getn() const{ 
	return N; 
} 

// Compute the norm of a vector. 
Vector2 Vector2::normalize() const 
{ 
	float magnitude; 
	Vector2 newv; 
	magnitude = norm(); 
	if (abs(x) > magnitude * HUGENUMBER || 
		abs(y) > magnitude * HUGENUMBER ){ 
			cerr << "Attempting to take the norm of a zero 2D vector." << endl; 
	} 
	newv.x = x / magnitude; 
	newv.y = y / magnitude; 
	return newv; 
} 
Vector3 Vector3::normalize() const 
{ 
	float magnitude; 
	Vector3 newv; 
	magnitude = norm(); 
	if (abs(x) > magnitude * HUGENUMBER || 
		abs(y) > magnitude * HUGENUMBER || 
		abs(z) > magnitude * HUGENUMBER ){ 
			cerr << "Attempting to take the norm of a zero 3D vector." << endl; 
	} 
	newv.x = x / magnitude; 
	newv.y = y / magnitude; 
	newv.z = z / magnitude; 
	return newv; 
} 
Vector4 Vector4::normalize() const 
{ 
	float magnitude; 
	Vector4 newv; 
	magnitude = norm(); 
	if (abs(x) > magnitude * HUGENUMBER || 
		abs(y) > magnitude * HUGENUMBER || 
		abs(z) > magnitude * HUGENUMBER ){ 
			cerr << "taking the norm of a zero 4D vector." << endl; 
	} 
	newv.x = x / magnitude; 
	newv.y = y / magnitude; 
	newv.z = z / magnitude; 
	newv.w = 1.0; 
	return newv; 
} 
Vector Vector::normalize() const 
{ 
	float magnitude; 
	Vector newv(N); 
	magnitude = norm(); 
	int i; 

	for(i = 0; i < N; i++){ 
		newv.v[i] = v[i] / magnitude; 
		if(abs(v[i]) > magnitude * HUGENUMBER){ 
			cerr << "taking the norm of a zero" << N << " Vector" << endl; 
			break; 
		} 
	} 
	for(; i < N; i++){ 
		newv.v[i] = v[i] / magnitude; 
	} 
	return newv; 
} 

//  Normalize a 4D Vector's x, y, z components by the w component, and 
//  set the w component to 1.0 
Vector4 Vector4::wnorm() const 
{ 
	Vector4 newv; 
	if (w == 1.0) 
		return *this; 
	else if (fabs(w) < SMALLNUMBER) 
		cerr << "w-Normalizing vector [" << 
		x << " " << y << " " << z<< " " << w << "] w ~= 0!" << endl; 

	newv.x = x / w; 
	newv.y = y / w; 
	newv.z = z / w; 
	newv.w = 1.0; 
	return newv; 
} 

//  Set the components of a Vector according to the arguments 
void Vector2::set(float vx, float vy) 
{ 
	x = vx; 
	y = vy; 
} 
void Vector2::set(const Vector2 &v) 
{ 
	x = v.x; 
	y = v.y; 
} 
void Vector3::set(float vx, float vy, float vz) 
{ 
	x = vx; 
	y = vy; 
	z = vz; 
} 
void Vector3::set(const Vector3 &v) 
{ 
	x = v.x; 
	y = v.y; 
	z = v.z; 
} 
void Vector4::set(float vx, float vy, float vz, float vw) 
{ 
	x = vx; 
	y = vy; 
	z = vz; 
	w = vw; 
} 
void Vector4::set(const Vector4 &v) 
{ 
	x = v.x; 
	y = v.y; 
	z = v.z; 
	w = v.w; 
} 
void Vector::set(float *vx){ 
	int i; 
	for(i = 0; i < N; i++) 
		v[i] = vx[i]; 
} 
void Vector::set(const Vector &V){ 
	int i; 

	setsize(V.N); 
	for(i = 0; i < N; i++) 
		v[i] = V.v[i]; 
} 
void Vector::set(float vx, float vy){ 
	if(N < 2){ 
		cerr << "too many arguments for " << N << " Vector set" << endl; 
		exit(1); 
	} 

	v[0] = vx; 
	v[1] = vy; 
	int i; 
	for(i = 2; i < N; i++) 
		v[i] = 0; 
} 
void Vector::set(float vx, float vy, float vz){ 
	if(N < 3){ 
		cerr << "too many arguments for " << N << " Vector set" << endl; 
		exit(1); 
	} 

	v[0] = vx; 
	v[1] = vy; 
	v[2] = vz; 
	int i; 
	for(i = 3; i < N; i++) 
		v[i] = 0; 
} 
void Vector::set(float vx, float vy, float vz, float vw){ 
	if(N < 4){ 
		cerr << "too many arguments for " << N << " Vector set" << endl; 
		exit(1); 
	} 

	v[0] = vx; 
	v[1] = vy; 
	v[2] = vz; 
	v[3] = vw; 
	int i; 
	for(i = 4; i < N; i++) 
		v[i] = 0; 
}  

// Print a Vector to the standard output device. 
void Vector2::print() const 
{ 
	cout << "[" << x << " " << y << "]"; 
} 
void Vector2::print(int w, int p) const 
{ 
	cout << "[" << setw(w) << setprecision(p) << round(x, p) << " "; 
	cout << setw(w) << setprecision(p) << round(y, p) << "]"; 
} 
void Vector3::print() const 
{ 
	cout << "[" << x << " " << y << " " << z << "]"; 
} 
void Vector3::print(int w, int p) const 
{ 
	cout << "[" << setw(w) << setprecision(p) << round(x, p) << " "; 
	cout << setw(w) << setprecision(p) << round(y, p) << " "; 
	cout << setw(w) << setprecision(p) << round(z, p) << "]"; 
} 
void Vector4::print() const 
{ 
	cout << "[" << x << " " << y << " " << z << " " << w << "]"; 
} 
void Vector4::print(int w, int p) const 
{ 
	cout << "[" << setw(w) << setprecision(p) << round(x, p) << " "; 
	cout << setw(w) << setprecision(p) << round(y, p) << " "; 
	cout << setw(w) << setprecision(p) << round(z, p) << " "; 
	cout << setw(w) << setprecision(p) << round(w, p) << "]"; 
} 
void Vector::print() const 
{ 
	if(N == 0) 
		cout << "[]"; 
	else{ 
		int i; 
		cout << "[" << v[0]; 
		for(i = 1; i < N; i++) 
			cout << " " << v[i]; 
		cout << "]"; 
	} 
} 
void Vector::print(int w, int p) const 
{ 
	if(N == 0) 
		cout << "[]"; 
	else{ 
		int i; 
		cout << "[" << setw(w) << setprecision(p) << round(v[0], p); 
		for(i = 1; i < N; i++) 
			cout << " " << setw(w) << setprecision(p) << round(v[i], p); 
		cout << "]"; 
	} 
} 

// Compute the magnitude of a vector. 
float Vector2::norm() const 
{ 
	return sqrt(normsqr()); 
} 
float Vector3::norm() const 
{ 
	return sqrt(normsqr()); 
} 
float Vector4::norm() const 
{ 
	return sqrt(normsqr()); 
} 
float Vector::norm() const 
{ 
	return sqrt(normsqr()); 
} 

// Compute the squared magnitude of a vector. 
float Vector2::normsqr() const 
{ 
	return sqr(x) + sqr(y); 
} 
float Vector3::normsqr() const 
{ 
	return sqr(x) + sqr(y) + sqr(z); 
} 
float Vector4::normsqr() const 
{ 
	return sqr(x) + sqr(y) + sqr(z) + sqr(w); 
} 
float Vector::normsqr() const 
{ 
	int i; 
	float sumsqr = 0; 
	for(i = 0; i < N; i++) 
		sumsqr += sqr(v[i]); 

	return sumsqr; 
} 

// Unary negation of a vector 
Vector2 operator-(const Vector2& v1){ 
	Vector2 r(-v1.x, -v1.y); 
	return(r); 
} 
Vector3 operator-(const Vector3& v1){ 
	Vector3 r(-v1.x, -v1.y, -v1.z); 
	return(r); 
} 
Vector4 operator-(const Vector4& v1){ 
	Vector4 r(-v1.x, -v1.y, -v1.z, -v1.w); 
	return(r); 
} 
Vector operator-(const Vector& v1){ 
	Vector r(v1.N); 
	int i; 

	for(i = 0; i < v1.N; i++) 
		r[i] = v1.v[i]; 
	return(r); 
} 

//  Addition of 2 Vectors. 
Vector2 operator+(const Vector2& v1, const Vector2& v2) 
{ 
	Vector2 r; 
	r.x = v1.x + v2.x; 
	r.y = v1.y + v2.y; 
	return r; 
} 

Vector3 Vector3::operator+(const Vector3& v2) const 
{ 
	Vector3 r; 
	r.x = x + v2.x; 
	r.y = y + v2.y; 
	r.z = z + v2.z; 
	return r; 
} 

Vector4 Vector4::operator+(const Vector4& v2) const 
{ 
	Vector4 r; 
	r.x = x + v2.x; 
	r.y = y + v2.y; 
	r.z = z + v2.z; 
	r.w = w + v2.w; 
	return r; 
} 

Vector operator+(const Vector& v1, const Vector& v2) 
{ 
	if(v1.N != v2.N){ 
		cerr << "cannot add " << v1.N << " Vector to " << v2.N << " Vector" <<  
			endl; 
		exit(1); 
	} 
	Vector r(v1.N); 
	int i; 

	for(i = 0; i < v1.N; i++) 
		r[i] = v1.v[i] + v2.v[i]; 
	return r; 
} 

//  Subtract two Vectors. 
Vector2 operator-(const Vector2& v1, const Vector2& v2) 
{ 
	Vector2 r; 
	r.x = v1.x - v2.x; 
	r.y = v1.y - v2.y; 
	return r; 
} 
Vector3 operator-(const Vector3& v1, const Vector3& v2) 
{ 
	Vector3 r; 
	r.x = v1.x - v2.x; 
	r.y = v1.y - v2.y; 
	r.z = v1.z - v2.z; 
	return r; 
} 
Vector4 operator-(const Vector4& v1, const Vector4& v2) 
{ 
	Vector4 r; 
	r.x = v1.x - v2.x; 
	r.y = v1.y - v2.y; 
	r.z = v1.z - v2.z; 
	r.w = v1.w - v2.w; 
	return r; 
} 
Vector operator-(const Vector& v1, const Vector& v2) 
{ 
	if(v1.N != v2.N){ 
		cerr << "cannot subtract " << v2.N << " Vector from " << v1.N << 
			" Vector" << endl; 
		exit(1); 
	} 

	Vector r(v1.N); 
	int i; 

	for(i = 0; i < v1.N; i++) 
		r[i] = v1.v[i] - v2.v[i]; 
	return(r); 
} 

void Vector3::operator+=(const Vector3& v2)
{
	x += v2.x;
	y += v2.y;
	z += v2.z;
}

void Vector3::operator-=(const Vector3& v2)
{
	x -= v2.x;
	y -= v2.y;
	z -= v2.z;
}

// Product of vector and scalar 
Vector2 operator*(const Vector2& v, float s) 
{ 
	Vector2 r; 

	r.x = v.x * s; 
	r.y = v.y * s; 
	return r; 
} 
Vector2 operator*(float s, const Vector2& v) 
{ 
	Vector2 r; 

	r.x = v.x * s; 
	r.y = v.y * s; 
	return r; 
} 
Vector3 operator*(const Vector3& v, float s) 
{ 
	Vector3 r; 

	r.x = v.x * s; 
	r.y = v.y * s; 
	r.z = v.z * s; 
	return r; 
} 
Vector3 operator*(float s, const Vector3& v) 
{ 
	Vector3 r; 

	r.x = v.x * s; 
	r.y = v.y * s; 
	r.z = v.z * s; 
	return r; 
} 
Vector4 operator*(const Vector4& v, float s) 
{ 
	Vector4 r; 

	r.x = v.x * s; 
	r.y = v.y * s; 
	r.z = v.z * s; 
	r.w = v.w * s; 
	return r; 
} 
Vector4 operator*(float s, const Vector4& v) 
{ 
	Vector4 r; 

	r.x = v.x * s; 
	r.y = v.y * s; 
	r.z = v.z * s; 
	r.w = v.w * s; 
	return r; 
} 
Vector operator*(float s, const Vector& v1) 
{ 
	Vector r(v1.N); 
	int i; 

	for(i = 0; i < v1.N; i++) 
		r[i] = s * v1.v[i]; 
	return r; 
} 
Vector operator*(const Vector& v1, float s) 
{ 
	Vector r(v1.N); 
	int i; 

	for(i = 0; i < v1.N; i++) 
		r[i] = s * v1.v[i]; 
	return r; 
} 

// Inner product of two Vectors 
float operator*(const Vector2& v1, const Vector2& v2) 
{ 
	return(v1.x * v2.x + 
		v1.y * v2.y); 
} 
float operator*(const Vector3& v1, const Vector3& v2) 
{ 
	return(v1.x * v2.x + 
		v1.y * v2.y + 
		v1.z * v2.z); 
} 
float operator*(const Vector4& v1, const Vector4& v2) 
{ 
	return(v1.x * v2.x + 
		v1.y * v2.y + 
		v1.z * v2.z + 
		v1.w * v2.w); 
} 
float operator*(const Vector& v1, const Vector& v2) 
{ 
	if(v1.N != v2.N){ 
		cerr << "cannot take dot product of " << v1.N << " Vector with " <<  
			v2.N << " Vector" << endl; 
		exit(1); 
	} 

	int i; 
	float p = 0; 

	for(i = 0; i < v1.N; i++) 
		p += v1.v[i] * v2.v[i]; 
	return p; 
} 
//  Component-wise multiplication of two Vectors 
Vector2 operator^(const Vector2& v1, const Vector2& v2) 
{ 
	Vector2 r; 
	r.x = v1.x * v2.x; 
	r.y = v1.y * v2.y; 
	return r; 
} 
Vector3 operator^(const Vector3& v1, const Vector3& v2) 
{ 
	Vector3 r; 
	r.x = v1.x * v2.x; 
	r.y = v1.y * v2.y; 
	r.z = v1.z * v2.z; 
	return r; 
} 
Vector4 operator^(const Vector4& v1, const Vector4& v2) 
{ 
	Vector4 r; 
	r.x = v1.x * v2.x; 
	r.y = v1.y * v2.y; 
	r.z = v1.z * v2.z; 
	r.w = v1.w * v2.w; 
	return r; 
} 
Vector operator^(const Vector& v1, const Vector& v2) 
{ 
	Vector r(v1.N); 
	int i; 

	for(i = 0; i < v1.N; i++) 
		r[i] = v1.v[i] * v2.v[i]; 
	return r; 
} 

// Cross product of two Vectors 
Vector3 operator%(const Vector2& v1, const Vector2& v2) 
{ 
	Vector3 cp; 
	cp.x = 0; 
	cp.y = 0; 
	cp.z = v1.x * v2.y - v1.y * v2.x; 
	return (cp); 
} 
Vector3 operator%(const Vector3& v1, const Vector3& v2) 
{ 
	Vector3 cp; 
	cp.x = v1.y * v2.z - v1.z * v2.y; 
	cp.y = v1.z * v2.x - v1.x * v2.z; 
	cp.z = v1.x * v2.y - v1.y * v2.x; 
	return (cp); 
} 
Vector4 operator%(const Vector4& v1, const Vector4& v2) 
{ 
	cerr << "sorry, cross product of Vector4's not yet implemented" << endl; 
	exit(1); 

	Vector4 cp = v1;	// here only to avoid warning message from compiler 
	cp = v2; 
	return (cp); 
} 
Vector operator%(const Vector& v1, const Vector& v2) 
{ 
	if(v1.N != v2.N || v1.N < 3){ 
		cerr << "cannot take cross product of " << v1.N << " Vector with " << 
			v2.N << " Vector" << endl; 
		exit(1); 
	} 

	cerr << "sorry, (Vector % Vector) not implemented yet" << endl; 
	exit(1); 
	return v1; 
} 

// Divide a vector by a scalar. 
Vector2 operator/(const Vector2& v, float s) 
{ 
	Vector2 r; 
	r.x = v.x / s; 
	r.y = v.y / s; 
	return(r); 
} 
Vector3 operator/(const Vector3& v, float s) 
{ 
	Vector3 r; 
	r.x = v.x / s; 
	r.y = v.y / s; 
	r.z = v.z / s; 
	return(r); 
} 
Vector4 operator/(const Vector4& v, float s) 
{ 
	Vector4 r; 
	r.x = v.x / s; 
	r.y = v.y / s; 
	r.z = v.z / s; 
	r.w = 1.0; 
	return(r); 
} 
Vector operator/(const Vector& v1, float s) 
{ 
	Vector r(v1.N); 
	int i; 

	for(i = 0; i < v1.N; i++) 
		r[i] = v1.v[i] / s; 
	return r; 
} 

// Determine if two Vectors are identical. 
short operator==(const Vector2& one, const Vector2& two) 
{ 
	return((one.x == two.x) && (one.y == two.y)); 
} 
short operator==(const Vector3& one, const Vector3& two) 
{ 
	return((one.x == two.x) && (one.y == two.y) && (one.z == two.z)); 
} 
short operator==(const Vector4& one, const Vector4& two) 
{ 
	return((one.x == two.x) && (one.y == two.y) && (one.z == two.z)); 
} 
short operator==(const Vector& one, const Vector& two) 
{ 
	int n, N; 
	float *v, *V; 

	if(one.N >= two.N){ 
		N = one.N; 
		n = two.N; 
		V = one.v; 
		v = two.v; 
	} 
	else{ 
		N = two.N; 
		n = one.N; 
		V = two.v; 
		v = one.v; 
	} 

	int i; 
	for(i = 0; i < n; i++) 
		if(V[i] != v[i]) 
			return 0; 
	for(;i < N; i++) 
		if(V[i] != 0) 
			return 0; 

	return 1; 
} 

// assign one vector to another 
const Vector& Vector::operator=(const Vector& v2){ 
	int i; 

	if(N != v2.N){ 
		delete []v; 
		setsize(v2.N); 
	} 
	for(i = 0; i < N; i++) 
		v[i] = v2.v[i]; 

	return *this; 
} 
